/*
 *  BumbleBallReal.cpp
 *  
 *
 *  Created by Ali Nouri
 *	This version works with Chris's camera and can be used directly from RL-glue
 */


#ifdef	OS_LINUX
#include <sys/time.h>
#endif

#include <math.h>
#include <unistd.h>

#include <rlglue/Environment_common.h>/* env_ function prototypes and RL-Glue types */
#include <rlglue/utils/C/RLStruct_util.h> /* helpful functions for allocating structs and cleaning them up */

// legacy code to connect to aibo and camera
#include "./RL_network_shared.h"
#include "./RL_socket.h"


//#define	USE_BALL
#define	PRINT_DEBUG

#ifndef	USE_BALL
//puddles in case we don't have the ball
#define PUDDLES 2
#define PUDDLERADIUS 50
double P[PUDDLES][2][2];
#define dot(u,v)   ( u[0] * v[0] + u[1] * v[1] )

#endif




prevRLGlue::RL_client_socket aiboConnection;		//this connection sends action to the actor
prevRLGlue::RL_client_socket cameraConnection;		//this connection gets state from the camera
 
#define	CAMERA_ADDRESS	"10.0.0.48"
#define	CAMERA_PORT		9999
#define	AIBO_ADDRESS	"10.0.0.2"
#define	AIBO_PORT		1360
#define	EXECUTION_INTERVAL	1100		//this is in millisecond
#define	EXECUTION_FIRST_TIME	1300

#define	PI	3.141596
//------environment parameters
#define	BALL_RADIUS		85
#define	X_MIN			30
#define	X_MAX			600
#define	Y_MIN			-445
#define	Y_MAX			-30

#define	GOAL_X		80
#define	GOAL_Y		-80
#define	GOAL_THRESHOLD	50
#define	COLLISION_REWARD	-40
//----------------------------

observation_t current_state; 
reward_observation_terminal_t ro; 

int episodeAge; 
int prevAction = 0; 
char tmpStr[15] = {0}; 
char stateStr[49] = {0};

#ifdef USE_BALL
const int dimension = 5; 
double obs_mins [dimension] = {X_MIN, Y_MIN, -180, X_MIN, Y_MIN}; 
double obs_maxs [dimension] = {X_MAX,Y_MAX,180, X_MAX,Y_MAX}; 
#else
const int dimension = 3;
double obs_mins [dimension] = {X_MIN, Y_MIN, -180}; 
double obs_maxs [dimension] = {X_MAX,Y_MAX,180}; 
#endif


const observation_t* env_start(); 
const reward_observation_terminal_t* env_step(const action_t* a); 
void makeRandomStartingPoint(observation_t& o);
bool getStateFromCamera(observation_t& o); 

void printObservation(const observation_t& o)
{
	printf("["); 
	for(int i=0; i< dimension; i++)
		printf("%lf,", o.doubleArray[i]); 
	printf("]\n"); 

}


void convertDouble(double* d)
{
	char* tmp = (char*) d; 
	for(int i=0; i< 4; i++)
	{
		char t = tmp[i]; 
		tmp[i] = tmp[7-i]; 
		tmp[7-i] = t; 
	}
}

void convertInt(int* d)
{
	char* tmp = (char*) d; 
	for(int i=0; i< 2; i++)
	{
		char t = tmp[i]; 
		tmp[i] = tmp[3-i]; 
		tmp[3-i] = t; 
	}
}

int getNextDelay(int a)
{
	if (a == prevAction)
		return EXECUTION_INTERVAL; 
	else
		return EXECUTION_FIRST_TIME; 
	
}

bool connectToAibo(int max_attempts)
{
	printf("Connecting to aibo at %s ...\n", AIBO_ADDRESS); 
	int attempt =0;
	int flag = 1; 
	while(!aiboConnection.is_connected() && ( attempt < max_attempts  || max_attempts==-1) )
	{
		if (!aiboConnection.connect_ip( AIBO_ADDRESS, AIBO_PORT ))
		{
		  printf("Unable To Connect to Aibo. Retrying in 10 seconds...\n");
		  attempt += 1;
		  sleep(10);
		}
	}

	if ( attempt == max_attempts )
	{
		printf("Server is unreachable. Please check the ip and port, or try again later.\n");
		return false; 
	}
	printf(" connected to Aibo\n"); 
	if (setsockopt(aiboConnection.get_socket(), IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int)) != 0)
		printf("could not set the options for aibo connection\n"); 
	
	if (setsockopt(aiboConnection.get_socket(), IPPROTO_TCP, SO_KEEPALIVE, (char*)&flag, sizeof(int)) != 0)
		printf("could not set aiboConnection to be keepalive\n"); 

	return true; 
}


bool connectToCamera(int max_attempts)
{
	printf("Connecting to camera at %s ...\n", CAMERA_ADDRESS); 
	int flag = 1; 
	int attempt =0;
	while(!cameraConnection.is_connected() && (attempt < max_attempts || max_attempts == -1))
	{
		if (!cameraConnection.connect_ip( CAMERA_ADDRESS, CAMERA_PORT ))
		{
		  printf("Unable To Connect to Camera. Retrying in 10 seconds...\n");
		  attempt += 1;
		  sleep(10);
		}
	}
	if ( attempt == max_attempts )
	{
		printf("Server is unreachable. Please check the ip and port, or try again later.\n");
		return false;  
	}
	printf("connected to Camera\n"); 
	setsockopt(cameraConnection.get_socket(), IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int));
	return true; 
}



double getAiboBatteryLevel()
{
	int size = 7;
	char  bat [8] = {0}; 
	sprintf(bat, "battery"); 
	aiboConnection.send(bat, size); 
	double level = 0; 
	aiboConnection.recv(level, 1); 
	convertDouble(&level); 

	return level; 
}




const char*  env_init()
{   
	allocateRLStruct(&current_state, 0, dimension, 0); 

	episodeAge = 0; 
	const int max_attempts = 5; 

	if (! connectToAibo(max_attempts))
		return 0; 

	if (! connectToCamera(max_attempts))
		return 0; 


/*
	while(true)
	{
		getStateFromCamera(current_state); 

	}
*/

/*
	double level = getAiboBatteryLevel(); 
	printf("the battery level is %lf\n", level); 


	printf("some simple simulation\n"); 
	env_start(); 
	printf("after env_start\n"); 
	for(int i=0; i< 20; i++)
	{
		printf("applying action %d\n", i); 
		ro = env_step(i%5); 
		if (ro.terminal)
			break; 
	}

*/

	char* ret = new char[1000];
#ifdef USE_BALL
  sprintf(ret, "VERSION RL-Glue-3.0 PROBLEMTYPE episodic DISCOUNTFACTOR 1.0 OBSERVATIONS DOUBLES (%lf %lf) (%lf %lf) (%lf %lf) (%lf %lf) (%lf %lf) ACTIONS INTS (0 5)  REWARDS (%lf 0) EXTRA ali"  , X_MIN, X_MAX, Y_MIN, Y_MAX, -180, 180, X_MIN, X_MAX, Y_MIN, Y_MAX, COLLISION_REWARD); 
#else
  sprintf(ret, "VERSION RL-Glue-3.0 PROBLEMTYPE episodic DISCOUNTFACTOR 1.0 OBSERVATIONS DOUBLES (%lf %lf) (%lf %lf) (%lf %lf) ACTIONS INTS (0 5)  REWARDS (%lf 0) EXTRA ali"  , X_MIN, X_MAX, Y_MIN, Y_MAX, -180, 180, COLLISION_REWARD); 
#endif

	return ret; 
}






bool is_terminalEX(const observation_t& st, double threshold)
{
	double dist = fabs(st.doubleArray[0] - GOAL_X) +  fabs(st.doubleArray[1] - GOAL_Y); 

	if ( dist < threshold)
		 return true; 
	else
		return false;
}

bool is_terminal(const observation_t& st)
{
	return is_terminalEX(st, GOAL_THRESHOLD); 
}


bool is_collision(const observation_t& obs)
{
#ifndef	USE_BALL
	return false; 
#endif


	double dist = fabs(obs.doubleArray[0] - obs.doubleArray[3] ) +	 fabs(obs.doubleArray[1] - obs.doubleArray[4]) ; 
//	printf("dist from ball is %lf\n", dist); 

	if ( dist < BALL_RADIUS)
		 return true; 
	else
		return false; 
}

#ifndef	USE_BALL
double pud_distance(double* state, int p)
{
  double v[2];
  double w[2];
  double z[2];
  double d; 

  for(int i = 0; i < 2; ++i) {
    v[i] = P[p][1][i] - P[p][0][i];
    w[i] = state[i] - P[p][0][i];
  }

  double c1 = dot(w,v);
  if ( c1 <= 0 ) {
    d = 0.0; 
    for(int i = 0; i < 2; ++i)
      d += (state[i] - P[p][0][i]) * (state[i] - P[p][0][i]);
    d = sqrt(d); 
    return d; 
  }

  double c2 = dot(v,v);
  if ( c2 <= c1 ) {
    d = 0.0;
    for(int i = 0; i < 2; ++i)
      d += (state[i] - P[p][1][i]) * (state[i] - P[p][1][i]);
    d = sqrt(d); 
    return d; 
  }

  double b = c1 / c2;
  d = 0.0;
  for(int i = 0; i < 2; ++i) {
    z[i] =  P[p][0][i] + b * v[i];
    d += (state[i] - z[i]) * (state[i] - z[i]);
  }
  d = sqrt(d); 
  return d;
}
#endif


double get_reward(const observation_t& state)
{
	if (is_terminal(state))
		return 0; 

#ifdef	USE_BALL
	//colision
	if ( is_collision(state) ) 
		return COLLISION_REWARD; 
	else
		return -1; 
#else

    P[0][0][0] = 200; //puddle #1, point #1, x
    P[0][0][1] = -200; //puddle #1, point #1, y
    P[0][1][0] = 400; //puddle #1, point #2, x
    P[0][1][1] = -200; //puddle #1, point #2, y

    P[1][0][0] = 250; //puddle #2, point #1, x
    P[1][0][1] = -200; //puddle #2, point #1, y
    P[1][1][0] = 250; //puddle #2, point #2, x
    P[1][1][1] = -300; //puddle #2, point #2, y
   	   


	//this is a regular stationary puddle: 
  double r; 
  int p; 
  double d; 

  // standard penalty
  r = -1; 

  // puddle penalty
  for (p=0;p<PUDDLES;p++) {
	  d = pud_distance(state.doubleArray, p);
    if ( d < PUDDLERADIUS)
    r += -1 * (PUDDLERADIUS - d); 
  }

  if (is_terminal(state))
    return 0;
  
  return r;

#endif
}



//retrieves the state information from the camera in a blocking fashion


bool getStateFromCamera(observation_t& st)
{
//	tmpStr = {0}; 
	sprintf(tmpStr, "q"); 
	if (cameraConnection.is_connected())
	{
		cameraConnection.send(tmpStr, strlen(tmpStr)); 
		cameraConnection.recv(stateStr, 48); 


//		printf("state received from camera: '%s' \n", stateStr); 


		//parse the string: 
		int rid , bid; 
		double bx, by; 
		sscanf(stateStr, "d%dx%lfy%lft%lfd%dx%lfy%lf", &rid, &st.doubleArray[0], &st.doubleArray[1], &st.doubleArray[2], &bid, &bx, &by);  
#ifdef USE_BALL
		st.doubleArray[3] = bx; 
		st.doubleArray[4] = by; 
#endif
		
		//the orientation is sent X10
		st.doubleArray[2] /= 10.0; 


#ifdef	USE_BALL
		double dist = sqrt((st.doubleArray[3] - st.doubleArray[0])*(st.doubleArray[3] - st.doubleArray[0]) + (st.doubleArray[4] - st.doubleArray[1])*(st.doubleArray[4] - st.doubleArray[1]));
		double gdist = fabs(st.doubleArray[0] - GOAL_X) + fabs(st.doubleArray[1] - GOAL_Y); 
		printf("state: [%3.1lf,%3.1lf,%3.1lf,%3.1lf,%3.1lf]\tcoldist: %3.1lf\tgoldist: %3.1lf \n", st.doubleArray[0], st.doubleArray[1], st.doubleArray[2], st.doubleArray[3], st.doubleArray[4], dist, gdist); 
#else
		double gdist = fabs(st.doubleArray[0] - GOAL_X) + fabs(st.doubleArray[1] - GOAL_Y); 
		printf("state: [%3.1lf,%3.1lf,%3.1lf]\tgoldist: %3.1lf \n", st.doubleArray[0], st.doubleArray[1], st.doubleArray[2], gdist); 
			
#endif


	//bring to bound
	for(int i=0; i< dimension ; i++)
	{
		if (st.doubleArray[i] > obs_maxs[i])
			st.doubleArray[i] = obs_maxs[i]; 
		
		if (st.doubleArray[i] < obs_mins[i])
			st.doubleArray[i] = obs_mins[i]; 
	}


		return true; 
	}

	printf("can't receive state from camera b/c connection is lost. reconnecting...\n"); 
	if ( connectToCamera(-1))
	{
		return getStateFromCamera(st); 
	}
	return false; 
}



bool sendActionToAibo(int a)
{
	printf("first %d %d \n", episodeAge, a); 
	sprintf(tmpStr, "%d %d\n", episodeAge, a); 
#ifdef	PRINT_DEBUG
	printf("time:action %d:\t%d\t",  episodeAge, a); 
#endif
	printf("second\n"); 
	if (aiboConnection.is_connected())
	{
		printf("1\n"); 
		int d = aiboConnection.send(tmpStr, strlen(tmpStr)); 
		printf("2\n"); 
		//now receive the sync 
		sprintf(tmpStr, "%d", episodeAge); 
		int len = strlen(tmpStr) + 5; 
		memset(tmpStr, 0, sizeof(char)*15); 
		aiboConnection.recv(tmpStr, len); 

		printf("3\n"); 
		char syncMessage[5] = {0};
		int syncAge = 0; 
		sscanf(tmpStr, "%d %s", &syncAge, syncMessage); 
		
		if (strcmp(syncMessage, "NORM")==0)
			return true; 
		else if (strcmp(syncMessage, "PAUS") == 0)
		{
			printf("pausing the simulation upon Aibo request\n"); 
			aiboConnection.close(); 			
			
			if (connectToAibo(-1))
				return sendActionToAibo(a); 
		}
	
	}

	printf("can't send action b/c connection is lost. reconnecting...\n");
	if (connectToAibo(-1))
	{
		return sendActionToAibo(a);  
	}
	return false; 
}


void guideToStartingPoint(observation_t& ppos)
{
	double* pos = ppos.doubleArray;

	episodeAge = 0; //go to negative -20

	double POS_THRESHOLD = 50; 
	double ANG_THRESHOLD = 30; 

	printf("going to location: \n"); 
	printObservation(ppos); 

	while(episodeAge-- > -30)
	{
		usleep(EXECUTION_INTERVAL*1000); 
		getStateFromCamera(current_state); 

		double dist = (pos[0] - current_state.doubleArray[0])*(pos[0] - current_state.doubleArray[0]) + (pos[1] - current_state.doubleArray[1])*(pos[1] - current_state.doubleArray[1]); 
		dist = sqrt(dist); 

		if (dist > POS_THRESHOLD)	//we should first go to that position
		{
			printf("so far away from the goal: %lf\n", dist); 
			//compute the directed angle to goal
			double targetAngle; 
			double xdiff = pos[0] - current_state.doubleArray[0]; 
			double ydiff = pos[1] - current_state.doubleArray[1]; 
			if (xdiff !=0)
			{
				targetAngle= ydiff/xdiff; 
				printf("tan is %lf\n", targetAngle); 
				targetAngle = atan(targetAngle); 
				printf("raw targetAngle is %lf\n", targetAngle); 

				if (targetAngle > 0 && xdiff < 0)
					targetAngle -= PI; 
				else if (targetAngle < 0 && xdiff < 0)
					targetAngle += PI; 

				targetAngle *= 57.30; 
			}else { //the angle is either 90 or -90
				int sign =(ydiff)>=0 ? 1:-1;
				targetAngle = 90*sign;
			}

			printf("targetAngle is %lf mine is: %lf\n", targetAngle, current_state.doubleArray[2]); 

			double angDiff = targetAngle - current_state.doubleArray[2]; 
			if (angDiff > 360) angDiff -= 360; 
			if (angDiff < -360) angDiff += 360; 
			if (fabs(angDiff)> 45)//we should turn to align ourselves to the goal
			{
				printf("we should first turn to align to goal\n");
				printf("difAngle is %lf\n", angDiff); 
				if (fabs(angDiff) > 180)
					angDiff = -angDiff; 
				int signa =(angDiff)>=0 ? 1:-1;
				sendActionToAibo (2 + signa); //turn right and left are (1,3) respectively
				continue; 
			}else
			{
				printf("going toward the goal\n"); 
				sendActionToAibo(0); 
				printf("action sent\n"); 
				continue; 
			}
		}else	//we'r at the right place. probably wrong angle
		{
			printf("at the goal, perhaps wrong angle\n"); 
			double angDiff = pos[2] - current_state.doubleArray[2]; 
			if (fabs(angDiff)> ANG_THRESHOLD)
			{
				if (fabs(angDiff) > 180)
					angDiff = -angDiff; 

				int sign =(angDiff)>=0 ? 1:-1;
				sendActionToAibo (2 + sign); //turn right and left are (1,3) respectively
				continue; 
			}
			//we're at the target, but if we're colliding with the ball, select another random point
			if (is_collision(current_state))
			{
				makeRandomStartingPoint(ppos); 
				guideToStartingPoint(ppos); 
				return; 
			}
			
			printf("successfully guided to the target\n"); 
			break; 
		}
	

				


		
	}

}

double floatRand(double lb, double ub)
{
	return drand48() * (ub-lb) + lb;
}


void makeRandomStartingPoint(observation_t& o)
{
	for(int cntr=0; cntr<40; cntr++)
	{
		o.doubleArray[0] = floatRand(obs_mins[0],obs_maxs[0]); 
		o.doubleArray[1] = floatRand(obs_mins[1],obs_maxs[1]); 
		o.doubleArray[2] = floatRand(-180, 180);

#ifndef	USE_BALL
		if (get_reward(o) < -1)
			continue; 
#endif

		if (!is_terminalEX(o, GOAL_THRESHOLD*1.4))	//we don't want to start too close to the goal
		{
			return;
		}

	}
}

//sends a shutdown command to aibo
void sendShutdown()
{
	int size = 7;
	char  bat [8] = {0}; 
	sprintf(bat, "shutdwn"); 
	aiboConnection.send(bat, size); 
}

bool checkBattery()
{
	//check for the battery level
	double level = getAiboBatteryLevel(); 
	if (level < 0.20 && level > 0.01)
	{
		if (aiboConnection.is_connected())
		{
			printf("battery level low(%lf); shutdown aiboConnectionn \n", level); 
			sendShutdown(); 
			aiboConnection.close(); 
		}
		printf("reconnecting to aibo\n"); 
		if (! connectToAibo(-1))
			return false; 
	}
	return true; 
}


const observation_t* env_start()
{
	
//	if (!checkBattery())
//		return 0; 
	printf("env starting\n"); 
	

	observation_t startPos; 
	allocateRLStruct(&startPos, 0, dimension, 0); 
	makeRandomStartingPoint(startPos); 

	printf("guiding\n"); 
	guideToStartingPoint(startPos); 
	episodeAge = 0; 
	printf("done\n"); 
	if (getStateFromCamera(current_state))
	{
			printf("returning...\n");
			clearRLStruct(&startPos); 
			 return &current_state; 
	}else
	{
		printf("Could not get the state from the camera\n"); 
		clearRLStruct(&startPos); 
		return 0; 
	}
}

const reward_observation_terminal_t* env_step(const action_t* aa)   
{
	printf("env steping\n"); 
	int a = aa->intArray[0]; 
	episodeAge++; 

	//execute agent action

/*	if (episodeAge% 30 == 29)
		if (!checkBattery())
			return &ro; 
*/	
	
	if (!sendActionToAibo(a))
	{
		printf("could not send the command to Aibo\n"); 
		return &ro; 
	}

	
	usleep(getNextDelay(a)*1000); 
	prevAction = a; 

	//now we can get the picture
	if (getStateFromCamera(current_state))
	{
		ro.observation = &current_state; 
		ro.reward = get_reward(current_state); 
		ro.terminal = is_terminal(current_state); 
		return &ro; 
	}else
	{
		printf("can not get camera image\n"); 
		ro.observation = 0; 
		ro.reward = 0; 
		ro.terminal = true; 
		return &ro; 
	}

}

void env_cleanup()
{
	if (aiboConnection.is_connected() )
		aiboConnection.close(); 

	if (cameraConnection.is_connected())
		cameraConnection.close(); 


	clearRLStruct(&current_state); 

}

const char* env_message(const char* inMessage) {
	if(strcmp(inMessage,"what is your name?")==0)
		return "my name is BumbleballReal!";

	return "I don't know how to respond to your message";
}

